#include <highgui.h>
#include <cv.h>
#include <cvaux.h>

#include <iostream>

using namespace std;
using namespace cv;

const Scalar RED = Scalar(0,0,255);
const Scalar PINK = Scalar(230,130,255);
const Scalar BLUE = Scalar(255,0,0);
const Scalar LIGHTBLUE = Scalar(255,255,160);
const Scalar GREEN = Scalar(0,255,0);

void draw_square(CvScalar color,int  d, IplImage *image, int x_pos, int y_pos)
{
  cvLine( image, cvPoint( x_pos - d, y_pos + d ),			\
	  cvPoint( x_pos + d, y_pos + d ), color,3, 0 );
  cvLine( image, cvPoint( x_pos + d, y_pos + d ),			\
	  cvPoint(  x_pos + d,  y_pos - d ), color, 3, 0 );
  cvLine( image, cvPoint( x_pos + d, y_pos - d ),			\
	  cvPoint(  x_pos - d,  y_pos - d ), color, 3, 0 );
  cvLine( image, cvPoint( x_pos - d, y_pos - d ),			\
	  cvPoint(  x_pos - d,  y_pos + d ), color, 3, 0 );

  cvLine( image, cvPoint( x_pos + 5, y_pos ),			\
	  cvPoint(  x_pos - 5,  y_pos  ), color, 2, 0 );
  cvLine( image, cvPoint( x_pos , y_pos - 5 ),			\
	  cvPoint(  x_pos ,  y_pos + 5 ), color, 2, 0 ); 

}

int main(void)
{
    CvCapture *OpenCV_CAM;

    OpenCV_CAM = cvCaptureFromCAM(-1);

    IplImage* src;
    IplImage *segmented;         // contour edges
    IplImage *tmp;     // thresholded image
    IplImage *gray;
    IplImage *edge;
    IplImage* colorEdges;
    IplImage* dst;
    IplImage* hsv;

    // COntour fitting
    CvMemStorage* storage = cvCreateMemStorage(0);
    CvPoint pt0, pt;

    CvBox2D box;
    CvPoint2D32f box_vtx[4];

    if (!OpenCV_CAM)
      {
	cout << " Error with capturing device!!" << endl;
	exit (-1);
      }

    //cvSetCaptureProperty( OpenCV_CAM, CV_CAP_PROP_FRAME_HEIGHT, 240);
    //cvSetCaptureProperty( OpenCV_CAM, CV_CAP_PROP_FRAME_WIDTH, 320 );

    src = cvQueryFrame( OpenCV_CAM );

    const string winName = "image";
    cvNamedWindow( winName.c_str(), CV_WINDOW_AUTOSIZE );
    
    colorEdges = cvCreateImage( cvGetSize(src), 8, 3 );
    dst = cvCreateImage( cvGetSize(src), 8, 3 );
    hsv = cvCreateImage( cvGetSize(src), 8, 3 );
    edge = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
    tmp = cvCreateImage (cvGetSize(src),IPL_DEPTH_8U,3);
    gray = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);

    // Convert to grayscale
    segmented = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 3);

    for(;;)
    {
        cvWaitKey(10);
	
	src = cvQueryFrame( OpenCV_CAM );

	cvCvtColor(src, gray, CV_BGR2GRAY);
	cvCvtColor(src, hsv, CV_BGR2HSV);
	
	// Run the edge detector on grayscale	 
	cvCanny(gray, edge, 70/*min*/, 120/*max*/, 3);
	cvZero( colorEdges );
	
	cvDilate(edge,edge,NULL,5);
	//cvErode(edge,edge, NULL,5);

	// copy edge points
	cvCopy( hsv, colorEdges, edge );

	//cvCopy( cedge, colorEdges );

	cvCopy( src, dst );

	//Params
	int posX = 200; //src->width/2;
	int posY = 200;  //src->height/2;
	int width = 100;

	// Fitting
	CvSeq* ptseq = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvContour),
				    sizeof(CvPoint), storage );
	uchar R, G, B;

	R = 0;
	G = 0;
	B = 0;

	//Init to 0
	for(int iY = posY-width; (iY < posY+width); iY++) 
	  {
	    for(int iX = posX-width; (iX < posX+width); iX++)
	      {
		((uchar *)(dst->imageData))[(iX+iY* src->width)*3] = (uchar)(0);
		((uchar *)(dst->imageData))[(iX+iY* src->width)*3+1] = (uchar)(0);
		((uchar *)(dst->imageData))[(iX+iY* src->width)*3+2] = (uchar)(0);

		((uchar *)(segmented->imageData))[(iX+iY* src->width)*3] = (uchar)(0);
		((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+1] = (uchar)(0);
		((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+2] = (uchar)(0);
	      }
	  }
	    
	  // seeding!!
	  for(int iY = posY-1; (iY < posY+1); iY++) 
	    {
	      for(int iX = posX-1; (iX < posX+1); iX++)
		{
		  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3] = (uchar)(255);
		  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+1] = (uchar)(255);
		  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+2] = (uchar)(0);
		}
	  }   

	  int totalPoints = -1;

	  for (int ii=0;ii<width-1;ii++)
	    {
	      cvDilate(segmented,segmented,NULL,1);

	      int points = 0;

	      for(int iY = posY-width; (iY < posY+width); iY++) 
		{
		  for(int iX = posX+width; (iX > posX-width); iX--)
		    {		
		      if ( ((uchar *)(edge->imageData))[(iX+iY*src->width)] == 255 )
			{
			  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3] = (uchar)(0);
			  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+1] = (uchar)(0);
			  ((uchar *)(segmented->imageData))[(iX+iY* src->width)*3+2] = (uchar)(0);
			}

		      R = ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3];     	    
		      G = ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3+1];

		      if ( (R==255) & (G==255) )
			{
			  ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3+2]=255;
			
       			  points ++;
			}
		    }
		}

    
	      if ( (totalPoints == points) & (points > 0) )
		break;
	      else
		totalPoints = points;
	    }

	  //defining contour
	  for(int iY = posY-width; (iY < posY+width); iY++) 
	    {
	      for(int iX = posX+width; (iX > posX-width); iX--)
		{		
		  if ( ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3+2] == 255 )
		    {
		      pt0.x = iX;
		      pt0.y = iY;
		      
		      cvSeqPush( ptseq, &pt0 );
		    }
		}
	    }

	//Clean up segmentation
	for(int iY = posY-width; (iY < posY+width); iY++) 
	  {
	    for(int iX = posX+width; (iX > posX-width); iX--)
	      {
		
		R = ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3];		    
		G = ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3+1];
		B = ((uchar *)(segmented->imageData))[(iX+iY*src->width)*3+2];
		
		if ((B == 255)) 
		  {
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3] = (uchar)(0);
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3+1] = (uchar)(155);
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3+2] = (uchar)(155);
		  }
		else
		  {
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3] = ((uchar *)(src->imageData))[(iX+iY* src->width)*3];
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3+1] = ((uchar *)(src->imageData))[(iX+iY* src->width)*3+1];
		    ((uchar *)(dst->imageData))[(iX+iY* src->width)*3+2] =  ((uchar *)(src->imageData))[(iX+iY* src->width)*3+2];
		  }
	      }
	 }

	//Fit
	if (ptseq->total > 0 )
	  {
	    box = cvMinAreaRect2( ptseq, 0 );
	    cvBoxPoints( box, box_vtx );

	    pt0.x = cvRound(box_vtx[3].x);
	    pt0.y = cvRound(box_vtx[3].y);

	    for( int i = 0; i < 4; i++ )
	      {
		pt.x = cvRound(box_vtx[i].x);
		pt.y = cvRound(box_vtx[i].y);

		cvLine(dst, pt0, pt, CV_RGB(0, 255, 0), 2, CV_AA, 0);
		pt0 = pt;
	      }
	  }
	
	// Draw square
	draw_square(RED,width, dst, posX,posY);
	
	cvShowImage( "image", dst );	    
     }
}
